/*
 * NeuroAuto.cpp
 *
 *  Created on: Jan 28, 2009
 *      Author: robotics
 *  
 */
#include "NeuroAuto.hh"

#define LEFT (90)
#define RIGHT (-90)
#define HALF_LEFT (45)
#define HALF_RIGHT (-45)

/*
 * Default constructor
 */
NeuroAuto::NeuroAuto(){
	
}

NeuroAuto::NeuroAuto(RobodoxJaguar *l, RobodoxJaguar *r, Encoder *lg, Encoder *rg, Encoder *rref, Encoder *lref, RobodoxTimer *rt, Gyro *g){  //YAP:  took out lr/rr
	leftMotor = l;
	rightMotor = r;
	
	leftGearbox = lg;
	rightGearbox = rg;
    rightRef = rref;
	leftRef = lref;
			
    time = rt;
		
	g1 = g;
	
	sum = 0;
	timeArray = (float *)calloc(15, sizeof(float));
	arrayLength = 15;
	i = autoIndex = 0;
	begin = false;

	srand((int)time->Get() * 1000);

	// generate random time intervals with total of at least 15 seconds
	for(i = 0; i < 15; i++){
	   timeArray[i] = randRange(1.0, 4.0);
	   sum += timeArray[i];
	}
	
	for(i = 0; i < 15; i++){
	   if(timeArray[i] == 0){
		   arrayLength = i;
		   break;
	   }
	}

	// generate random motor speeds for each time interval	
	speedArray = (float *)calloc(arrayLength, sizeof(float));
	for(i = 0; i < arrayLength; i++){
	   speedArray[i] = randRange(-1.0, 1.0);
	}
		
}
/*
 * Default destructor
 */
NeuroAuto::~NeuroAuto(){
	delete leftMotor;
	delete rightMotor;
		
	delete leftGearbox;
	delete rightGearbox;

	delete rightRef;
	delete leftRef;
	
	delete time;
	
	delete g1;
}


void NeuroAuto::RandomDrive(){
	if(time->Get() <= 1.5 && !begin){
		leftMotor->Set(1.0);
		rightMotor->Set(1.0);
	}

	if(time->Get() == 1.5) begin = true;

	if(time->Get() >= timeArray[autoIndex] && begin){
		time->Reset();
	    autoIndex++;
	}

	if(autoIndex % 2 == 0 && begin){
		leftMotor->Set(speedArray[autoIndex]);
		rightMotor->Set(speedArray[autoIndex]);
	}
	else if(autoIndex %2 != 0 && begin){
		if(randRange(0,2) == 1){
			leftMotor->Set(speedArray[autoIndex]);
			rightMotor->Set(-speedArray[autoIndex]);
		}
		else{
			leftMotor->Set(-speedArray[autoIndex]);
			rightMotor->Set(speedArray[autoIndex]);
		}
	}
}



/********************************************
 * 			Position possibilities			*
 * 											*
 * 	1 = middle								*
 * 	2 = corner (turn then short distance)	*
 * 	3 = corner (turn then long distance)	*
 *											*
 ********************************************/
								
/********************************************
 *	1) drive 3.16 ft;						*
 *	2) turn left 90 degrees;				*
 *	3) drive 20.25 ft;						*
 *	4) turn right 90 degrees;				*
 *	5) drive 13.5 ft;						*
 *	6) turn right 90 degrees;				*
 *	7) drive 40.5 ft;						*
 *	8) repeat 4-7 until end of autonomous.;	*
 ********************************************/
		
void NeuroAuto::AroundDrive(int position){

	char flag = 0;
	
	if(position == 1){
		if(flag == 0){
			GoForward(40);
			Turn(LEFT);
			GoForward(243);
			flag = 1;
		}
		Turn(RIGHT);
		GoForward(168);
		Turn(RIGHT);
		GoForward(486);
	}
	if(position == 2){
		if(flag == 0){
			GoForward((int)(40*(sqrt(2))));
			Turn(HALF_LEFT);
			flag = 1;
		}
		GoForward(168);
		Turn(RIGHT);
		GoForward(486);
		Turn(RIGHT);
	}
	if(position == 3){
		if(flag == 0){
			GoForward((int)(40*(sqrt(2))));
			Turn(HALF_LEFT);
			GoForward(486);
			flag = 1;
		}
		Turn(RIGHT);
		GoForward(168);
		Turn(RIGHT);
		GoForward(486);
	}
}
			
void NeuroAuto::GoForward(int inches){
	if(((rightRef->GetDistance()+rightRef->GetDistance())/2)<=inches){//486 is in INCHES
		leftMotor->Set(1.0);
		rightMotor->Set(1.0);
	}
}
		
// TODO: Might work but there's probably a better way to do it
void NeuroAuto::GoForward(double tim){		
	static double desiredTime, currentTime = 0; //This may be causing the problem. We need to know if the value keeps getting set to zero
	if(currentTime == 0){
		currentTime = time->Get();
	}
	desiredTime = currentTime + tim;
	if(time->Get() <= desiredTime){
		leftMotor->Set(1.0);
		rightMotor->Set(1.0);
	}
}

void NeuroAuto::Turn(int degrees){//-180 to 180 degrees rotation
	
	//Slow to a stop	
	Stop();						//disable this if accelerometer doesn't work.
				
	g1->Reset();
	
	if(v_robot == 0){
		if(degrees > 0){
			if(g1->GetAngle() <= degrees){
				leftMotor->Set(-1.0);
				rightMotor->Set(1.0);
			}
		}
		else if(degrees < 0){
			if(g1->GetAngle() >= degrees){
				leftMotor->Set(1.0);
				rightMotor->Set(-1.0);
			}
		}
	}
}
		
void NeuroAuto::Stop(){
	if(v_robot != 0){
		leftMotor->Set(0);
		rightMotor->Set(0);
	}
}
		
float NeuroAuto::randRange(float min, float max){
	min *= 1000;
	max *= 1000;
	return ((min + (rand( ) % (int)max)) / 1000);
}
		
void NeuroAuto::CenterCircling() {
	if(time->Get() < 1.5) {
		leftMotor->Set(1.0);		//drive forward
		rightMotor->Set(1.0);  
	}
	else if(time->Get() < 1.75) {
		leftMotor->Set(1.0);
		rightMotor->Set(-1.0);		//turn right
	}
	else if(time->Get() < 3.25) {
		leftMotor->Set(1.0);		//drive forward up fieldd
		rightMotor->Set(1.0);  
	}
	else if(time->Get() < 3.50){ 
		leftMotor->Set(-1.0);		//turn left
		rightMotor->Set(1.0);
	} 
	else if(time->Get() < 6.50) {
		leftMotor->Set(1.0);		//drive forward to other end of field
		rightMotor->Set(1.0);
	}
	else if(time->Get() >= 6.50){
		time->Set(3.50);			//rinse and repeat
	}
}
	
void NeuroAuto::CornerCircling() {	//from either the left/right corner
	if(time->Get() < 1.5) {
		leftMotor->Set(1.0);
		rightMotor->Set(1.0);		//drive forward
	}
	else if(time->Get() < 1.6) {
		leftMotor->Set(-1.0);		//turn left slightly to straighten
		rightMotor->Set(1.0);
	}
	else if(time->Get() < 4.6) {
		leftMotor->Set(1.0);
		rightMotor->Set(1.0);		//drive forward other end of field
	}
	else if(time->Get() < 4.85) {
	    leftMotor->Set(-1.0);
	    rightMotor->Set(1.0);		//turn left slightly
	}
	else if(time->Get() >= 4.85){
	    time->Set(4.6);				//rinse and repeat
	}
}
		
